#include <chrono>
#include <thread>
#include <iostream>
#include <csignal>
#include <unistd.h>
#include "motor_drivers.hpp"
#include "motor_sockets/can_socket.hpp"


int main(){
    // 新建一个CANSocket对象 can0是端口名，0x201是发送ID，0x200是接收ID，8是数据长度
    MotorDrivers::CANSocket m2006_s1("can0", 0x201, 0x200, 8);
    //新建MotorM2006对象，m2006_s1将被用作其端口
    auto m1 = std::make_shared<MotorDrivers::MotorM2006>(&m2006_s1);
    //设置电机模式：位置模式
    m1.get()->motor_SetMode(0);
    //设置目标位置
    m1.get()->motor_SetTargetPos(10.0);
    //位置环PID参数
    // m1.get()->pid_pos.kp = 4.9;
    // m1.get()->pid_pos.ki = 0.02;
    // m1.get()->pid_pos.kd = 0.3;
    //设置目标速度
    m1.get()->motor_SetPosTargetVel(50.0);
    //速度环PID参数
    // m1.get()->pid_speed.kp = 1.0;
    // m1.get()->pid_speed.ki = 0.1;
    // m1.get()->pid_speed.kd = 0.1;


    //M2006阵列需要两个CANSocket对象来分别各负责4个电机
    MotorDrivers::CANSocket c610_s1("can0", 0x200, 0x200, 8);
    MotorDrivers::CANSocket c610_s2("can0", 0x200, 0x1FF, 8);
    //新建一个MotorM2006对象的vector 使用智能指针管理其内存
    std::vector<std::shared_ptr<MotorDrivers::MotorM2006>> motors;
    motors.push_back(m1);

    //新建M2006_Matrix对象并将其初始化
    MotorDrivers::M2006_Matrix m2006(&motors, &c610_s1, &c610_s2);
    m2006.motor_Init();

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    while(true){
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    //m2006阵列更新,每个电机的位置和速度将被更新
        m2006.motor_Update();
    }

    return 0;
}